#include "Cone.h"

#include "math.h"

Cone::Cone(DAEFloat3 origin, DAEFloat3 axis, float angle,float startRange, float stopRange):
	m_Origin(origin),
	m_Axis(axis),
	m_Angle(angle),
	m_CosAngle2( cos(angle)*cos(angle)),
	m_StartRange(startRange),
	m_StopRange(stopRange)
{
	m_Axis.Normalize();
}


Cone::~Cone(void)
{
}

bool Cone::IsInside(DAEFloat3& p){
	DAEFloat3 diff = p- m_Origin;
	float dot = diff.Dot(m_Axis);	
	if ( dot < m_StartRange || dot > m_StopRange )
		return false;
	float dot2 = dot*dot;
	float right = m_CosAngle2 * diff.SquaredLength();
	return ( dot2 > right );

}

float Cone::TestPoint(DAEFloat3& p1){
	DAEFloat3 diff = p1 - m_Origin;
	float dot = diff.Dot(m_Axis) / diff.Length();	
	float angle = acos(dot);
	return angle-m_Angle;
}

bool Cone::IntersectsLine(LineSegment& line)
{
	return IntersectsLine(line.GetP1(),line.GetP2());
}

bool Cone::IntersectsLine(DAEFloat3& p1, DAEFloat3& p2)
{
	DAEFloat3 E0= p2 - p1;
	
	float AdotE0 = m_Axis.Dot(E0);
	DAEFloat3 Delta0 = p1 - m_Origin;
	float AdotDelta0 = m_Axis.Dot(Delta0);
	
	float c2 = AdotE0 * AdotE0 - m_CosAngle2 * E0.SquaredLength();
	float c1 = AdotE0 * AdotDelta0 - m_CosAngle2 * E0.Dot(Delta0);
	float c0 = AdotDelta0 * AdotDelta0 - m_CosAngle2 * Delta0.SquaredLength();
	

	// if both points are on the positive side of the plane defined by 
	// axis of the cone
	if ( c2 > 0 )
		return false;

	if ( c1 > -c2 || c1 < 0 )
		return false;
	
	return true;
}

bool Cone::IntersectsTriangle( DAEFloat3& p1, DAEFloat3& p2, DAEFloat3& p3)
{
	// check if points are within sphere defined by origin and range.
	DAEFloat3 diff1 = p1-m_Origin;
	DAEFloat3 diff2 = p2-m_Origin;
	DAEFloat3 diff3 = p3-m_Origin;

	float axisSide1 = m_Axis.Dot(diff1);
	float axisSide2 = m_Axis.Dot(diff2);
	float axisSide3 = m_Axis.Dot(diff3);
	if ( axisSide1 < 0 && axisSide2 < 0 && axisSide3 < 0 )
		return false;

	/*
	float m_StopRange2 =m_StopRange*m_StopRange;
	float m_StartRange2 = m_StartRange*m_StartRange;
	float difflength1 = diff1.SquaredLength();
	float difflength2 = diff2.SquaredLength();
	float difflength3 = diff3.SquaredLength();
	if ( (difflength1 > m_StopRange2 && difflength2> m_StopRange2 && difflength3 > m_StopRange2)
	|| (difflength1 < m_StartRange2 && difflength2 < m_StartRange2 && difflength3 < m_StartRange2)	)
	{
		return false;
	}*/
	
	
	if ( IsInside(p1))
		return true;
	if ( IsInside(p2))
		return true;
	if ( IsInside(p3))
		return true;
		
	/*
	if ( IntersectsLine(p1,p2))
		return true;
	if (IntersectsLine(p2,p3))
		return true;
	if ( IntersectsLine(p3,p1))
		return true;
	*/
	return false;
	//return AxisIntersectsTriangle(p1,p2,p3);
}


bool Cone::AxisIntersectsTriangle(DAEFloat3& v0, DAEFloat3& v1, DAEFloat3& v2)
{
	DAEFloat3 e1,e2,h,s,q;
	float a,f,u,v;
	
	e1 = v1 - v0;
	e2 = v2 - v0;
	
	m_Axis.Cross(h,e2);
	a = e1.Dot(h);
	
	if (a > -0.00001 && a < 0.00001)
		return(false);
	
	f = 1/a;
	s = m_Origin - v0;

	u = f * (s.Dot(h));
	
	if (u < 0.0 || u > 1.0)
		return(false);
	
	s.Cross(q,e1);
	v = f * m_Axis.Dot(q);
	if (v < 0.0 || u + v > 1.0)
		return(false);
	else
		return true;
	/*
	// at this stage we can compute t to find out where 
	// the intersection point is on the line
	float t = f * e2.Dot(q);
	if (t > 0.00001) // ray intersection
		return(true);
	else // this means that there is a line intersection  
		 // but not a ray intersection
		 return (false);
		 */
}

bool Cone::AxisIntersectPlane( DAEFloat3& result, DAEFloat3& origin, DAEFloat3& normal)
{
	float dirdotnormal = normal.Dot(m_Axis);
	if ( dirdotnormal > 0.00001f  || dirdotnormal < -0.00001f)
	{
		DAEFloat3 diff = origin - m_Origin;
		float diffdotnormal = diff.Dot(normal);
		float d = diffdotnormal / dirdotnormal;
		result =  m_Origin + m_Axis * d;
		return true;
	}else
		return false;
}

bool Cone::IntersectsQuad( DAEFloat3& p1, DAEFloat3& p2, DAEFloat3& p3, DAEFloat3& p4)
{
	if ( IntersectsTriangle(p1,p2,p3) )
		return true;
	else
		return IntersectsTriangle(p3,p4,p1);
}

bool Cone::IntersectsOBB(OBB& obb){
	DAEFloat3 b1, b2,b3,b4;
	DAEFloat3 u1,u2,u3,u4;

	b1 = obb.GetB1();
	b2 = obb.GetB2();
	b3 = obb.GetB3();
	b4 = obb.GetB4();
	
	u1 = obb.GetU1();
	u2 = obb.GetU2();
	u3 = obb.GetU3();
	u4 = obb.GetU4();

	if ( IntersectsQuad(b1,b2,b3,b4))
		return true;
	else if ( IntersectsQuad(u1,u2,u3,u4))
		return true;
	else if ( IntersectsQuad(b1,b2,u2,u1))
		return true;
	else if ( IntersectsQuad(b2,b3,u3,u2))
		return true;
	else if ( IntersectsQuad(b3,b4,u4,u3))
		return true;
	else return ( IntersectsQuad(b4,b1,u1,u4));
}


bool Cone::IntersectsTriangle2(DAEFloat3& p0,DAEFloat3& p1, DAEFloat3& p2)
{
	// Triangle is <P0,P1,P2>, edges are E0 = P1-P0, E1=P2-P0.
    int onConeSide = 0;
    float p0Test = 0, p1Test = 0, p2Test = 0;
    float AdE, EdE, EdD, c1, c2;

    float cosSqr =m_CosAngle2;

    // Test vertex P0.
    
	DAEFloat3 diff0 = p0 - m_Origin;
    float AdD0 = m_Axis.Dot(diff0);
    if (AdD0 >= 0)
    {
        // P0 is on cone side of plane.
        p0Test = AdD0*AdD0 - cosSqr*(diff0.Dot(diff0));
        if (p0Test >= 0)
        {
            // P0 is inside the cone.
            return true;
        }
        else
        {
            // P0 is outside the cone, but on cone side of plane.
            onConeSide |= 1;
        }
    }
    // else P0 is not on cone side of plane.

    // Test vertex P1.
    DAEFloat3 edge0 = p1 - p0;
    DAEFloat3 diff1 = diff0 + edge0;
    float AdD1 =m_Axis.Dot(diff1);
    if (AdD1 >= 0)
    {
        // P1 is on cone side of plane.
        p1Test = AdD1*AdD1 - cosSqr*(diff1.Dot(diff1));
        if (p1Test >= 0)
        {
            // P1 is inside the cone.
            return true;
        }
        else
        {
            // P1 is outside the cone, but on cone side of plane.
            onConeSide |= 2;
        }
    }
    // else P1 is not on cone side of plane.

    // Test vertex P2.
    DAEFloat3 edge1 = p2 - p0;
    DAEFloat3 diff2 = diff0 + edge1;
    float AdD2 = m_Axis.Dot(diff2);
    if (AdD2 >= 0)
    {
        // P2 is on cone side of plane.
        p2Test = AdD2*AdD2 - cosSqr*(diff2.Dot(diff2));
        if (p2Test >= 0)
        {
            // P2 is inside the cone.
            return true;
        }
        else
        {
            // P2 is outside the cone, but on cone side of plane.
            onConeSide |= 4;
        }
    }
    // else P2 is not on cone side of plane.

    // Test edge <P0,P1> = E0.
    if (onConeSide & 3)
    {
        AdE = AdD1 - AdD0;
        EdE = edge0.Dot(edge0);
        c2 = AdE*AdE - cosSqr*EdE;
        if (c2 < 0)
        {
            EdD = edge0.Dot(diff0);
            c1 = AdE*AdD0 - cosSqr*EdD;
            if (onConeSide & 1)
            {
                if (onConeSide & 2)
                {
                    // <P0,P1> fully on cone side of plane, fC0 = p0Test.
                    if (0 <= c1 && c1 <= -c2 && c1*c1 >= p0Test*c2)
                    {
                        return true;
                    }
                }
                else
                {
                    // P0 on cone side (Dot(A,P0-V) >= 0),
                    // P1 on opposite side (Dot(A,P1-V) <= 0)
                    // (Dot(A,E0) <= 0), fC0 = p0Test.
                    if (0 <= c1 && c2*AdD0 <= c1*AdE
                    &&  c1*c1 >= p0Test*c2)
                    {
                        return true;
                    }
                }
            }
            else
            {
                // P1 on cone side (Dot(A,P1-V) >= 0),
                // P0 on opposite side (Dot(A,P0-V) <= 0)
                // (Dot(A,E0) >= 0), fC0 = p0Test (needs calculating).
                if (c1 <= -c2 && c2*AdD0 <= c1*AdE)
                {
                    p0Test = AdD0*AdD0 - cosSqr*(diff0.Dot(diff0));
                    if (c1*c1 >= p0Test*c2)
                    {
                        return true;
                    }
                }
            }
        }
    }
    // else <P0,P1> does not intersect cone half space.

    // Test edge <P0,P2> = E1.
    if (onConeSide & 5)
    {
        AdE = AdD2 - AdD0;
        EdE = edge1.Dot(edge1);
        c2 = AdE*AdE - cosSqr*EdE;
        if (c2 < 0)
        {
            EdD = edge1.Dot(diff0);
            c1 = AdE*AdD0 - cosSqr*EdD;
            if (onConeSide & 1)
            {
                if (onConeSide & 4)
                {
                    // <P0,P2> fully on cone side of plane, fC0 = p0Test.
                    if (0.0 <= c1 && c1 <= -c2 && c1*c1 >= p0Test*c2)
                    {
                        return true;
                    }
                }
                else
                {
                    // P0 on cone side (Dot(A,P0-V) >= 0),
                    // P2 on opposite side (Dot(A,P2-V) <= 0)
                    // (Dot(A,E1) <= 0), fC0 = p0Test.
                    if (0.0 <= c1 && c2*AdD0 <= c1*AdE
                    &&  c1*c1 >= p0Test*c2)
                    {
                        return true;
                    }
                }
            }
            else
            {
                // P2 on cone side (Dot(A,P2-V) >= 0),
                // P0 on opposite side (Dot(A,P0-V) <= 0)
                // (Dot(A,E1) >= 0), fC0 = p0Test (needs calculating).
                if (c1 <= -c2 && c2*AdD0 <= c1*AdE)
                {
                    p0Test = AdD0*AdD0 - cosSqr*(diff0.Dot(diff0));
                    if (c1*c1 >= p0Test*c2)
                    {
                        return true;
                    }
                }
            }
        }
    }
    // else <P0,P2> does not intersect cone half space.

    // Test edge <P1,P2> = E1-E0 = E2.
    if (onConeSide & 6)
    {
        DAEFloat3 kE2 = edge1 - edge0;
        AdE = AdD2 - AdD1;
        EdE = kE2.Dot(kE2);
        c2 = AdE*AdE - cosSqr*EdE;
        if (c2 < 0.0)
        {
            EdD = kE2.Dot(diff1);
            c1 = AdE*AdD1 - cosSqr*EdD;
            if (onConeSide & 2)
            {
                if (onConeSide & 4)
                {
                    // <P1,P2> fully on cone side of plane, fC0 = p1Test.
                    if (0.0 <= c1 && c1 <= -c2 && c1*c1 >= p1Test*c2)
                    {
                        return true;
                    }
                }
                else
                {
                    // P1 on cone side (Dot(A,P1-V) >= 0),
                    // P2 on opposite side (Dot(A,P2-V) <= 0)
                    // (Dot(A,E2) <= 0), fC0 = p1Test.
                    if (0.0 <= c1 && c2*AdD1 <= c1*AdE
                    &&  c1*c1 >= p1Test*c2)
                    {
                        return true;
                    }
                }
            }
            else
            {
                // P2 on cone side (Dot(A,P2-V) >= 0),
                // P1 on opposite side (Dot(A,P1-V) <= 0)
                // (Dot(A,E2) >= 0), fC0 = p1Test (needs calculating).
                if (c1 <= -c2 && c2*AdD1 <= c1*AdE)
                {
                    p1Test = AdD1*AdD1 - cosSqr*(diff1.Dot(diff1));
                    if (c1*c1 >= p1Test*c2)
                    {
                        return true;
                    }
                }
            }
        }
    }
    // else <P1,P2> does not intersect cone half space.

    // Test triangle <P0,P1,P2>.  It is enough to handle only the case when
    // at least one Pi is on the cone side of the plane.  In this case and
    // after the previous testing, if the triangle intersects the cone, the
    // set of intersection must contain the point of intersection between
    // the cone axis and the triangle.
    if (onConeSide > 0)
    {
        DAEFloat3 N;
		edge0.Cross(N,edge1);
        float NdA = N.Dot(m_Axis);
        float NdD = N.Dot(diff0);
		DAEFloat3 left = m_Axis*NdD;
		DAEFloat3 right = diff0 *NdA;
        DAEFloat3 U = left-right;
        DAEFloat3 NcU;
		N.Cross(NcU,U);

        float NcUdE0 = NcU.Dot(edge0), NcUdE1, NcUdE2, NdN;
        if (NdA >= 0)
        {
            if (NcUdE0 <= 0)
            {
                NcUdE1 = NcU.Dot(edge1);
                if (NcUdE1 >= 0)
                {
                    NcUdE2 = NcUdE1 - NcUdE0;
                    NdN = N.SquaredLength();
                    if (NcUdE2 <= NdA*NdN)
                    {
                        return true;
                    }
                }
            }
        }
        else
        {
            if (NcUdE0 >= 0)
            {
                NcUdE1 = NcU.Dot(edge1);
                if (NcUdE1 <= 0)
                {
                    NcUdE2 = NcUdE1 - NcUdE0;
                    NdN = N.SquaredLength();
                    if (NcUdE2 >= NdA*NdN)
                    {
                        return true;
                    }
                }
            }
        }
    }

    return false;
}

int Cone::IntersectLine( DAEFloat3& p1, DAEFloat3& p2, DAEFloat3& ip1, DAEFloat3& ip2)
{
	DAEFloat3 E0= p2 - p1;
	
	float AdotE0 = m_Axis.Dot(E0);
	DAEFloat3 Delta0 = p1 - m_Origin;
	float AdotDelta0 = m_Axis.Dot(Delta0);
	
	float c2 = AdotE0 * AdotE0 - m_CosAngle2 * E0.SquaredLength();
	float c1 = AdotE0 * AdotDelta0 - m_CosAngle2 * E0.Dot(Delta0);
	float c0 = AdotDelta0 * AdotDelta0 - m_CosAngle2 * Delta0.SquaredLength();

	if ( abs(c2)  < 0.000001f  && abs(c1) < 0.000001f) 
		// degenerate
		return 0;
	else{
		if ( c2 > 0 )
			return 0;

		if ( c1 > -c2 || c1 < 0 )
			return 0;

		float det = sqrt(c1*c1 - c0*c2);

		float t1 = (-c1 + det) / (c2);
		float t2 = (-c1 - det) / (c2);

		if ( abs(t1 - t2 ) > 0.00001 )
		{
			ip1 = p1 + E0*t1;
			ip2 = p1 + E0*t2;
			return 2;
		}else{
			ip1 = p1 + E0*t1;
			return 1;
		}
	}
}

int Cone::IntersectLine( DAEFloat3& p1, DAEFloat3& p2, float& t1, float& t2,bool limitTs)
{
	DAEFloat3 E0= p2 - p1;
	
	float AdotE0 = m_Axis.Dot(E0);
	DAEFloat3 Delta0 = p1 - m_Origin;
	float AdotDelta0 = m_Axis.Dot(Delta0);
	
	float c2 = AdotE0 * AdotE0 - m_CosAngle2 * E0.SquaredLength();
	float c1 = AdotE0 * AdotDelta0 - m_CosAngle2 * E0.Dot(Delta0);
	float c0 = AdotDelta0 * AdotDelta0 - m_CosAngle2 * Delta0.SquaredLength();
	if ( abs(c1) < 0.0001 )
		c1 = 0;

	if ( abs(c2)  < 0.000001f  && abs(c1) < 0.000001f) 
		// degenerate
		return 0;
	else{
		if ( c2 > 0 )
			return 0;

		//if ( c1 > -c2 || c1 < 0 )
		//	return 0;

		if ( c1*c1 < c0*c2 )
			return 0;

		float det = sqrt(c1*c1 - c0*c2);

		float t1result = (-c1 + det) / (c2);
		float t2result = (-c1 - det) / (c2);

		if ( abs(t1result - t2result ) > 0.00001 )
		{
			t1 = t1result;
			t2 = t2result;
			int nrOfIntersections = 0;
			bool t1Valid = (t1 > .01 && t1 < 0.99) || !limitTs;
			bool t2Valid = (t2 > .01 && t2 < 0.99) || !limitTs;
			
			if( t1Valid ){
				t1 = t1result;
				if (t2Valid ){
					
					if (t2result < t1result ){
						t1 = t2result;
						t2 = t1result;
					}else
						t2 = t2result;
					return 2;
				}else
					return 1;
			}else if ( t2Valid ){
				t1 = t2result;
				return 1;
			}else
				return 0;
		}else{
			t1 = t1result;
			return 1;
		}
	}
}

bool Cone::IntersectTriangle( DAEFloat3& p0, DAEFloat3& p1, DAEFloat3& p2, float& l1, float& l2)
{
	DAEFloat3 e1,e2,h,s,q;
	float a,f,u,v;
	
	e1 = p1 - p0;
	e2 = p2 - p0;
	
	m_Axis.Cross(h,e2);
	a = e1.Dot(h);
	
	if (a > -0.00001 && a < 0.00001)
		return(false);
	
	f = 1/a;
	s = m_Origin - p0;

	u = f * (s.Dot(h));
	
	if (u < 0.0 || u > 1.0)
		return(false);
	
	s.Cross(q,e1);
	v = f * m_Axis.Dot(q);
	if (v < 0.0 || u + v > 1.0)
		return(false);
	else{
		l1 = u;
		l2 = v;
		return true;
	}
}
